/*
 * ============================================================================
 *
 *       Filename:  algorithm.c
 *
 *    Description:  
 *
 *        Version:  1.0
 *        Created:  2014年09月06日 09时30分44秒
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  jianxi sun (jianxi), ycsunjane@gmail.com
 *   Organization:  
 *
 * ============================================================================
 */
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

#include "algorithm.h"

struct matrix_A_t {
	double a[MAX_M][2];
};

struct matrix_A_1_t {
	double a[MAX_M][2];
};

struct matrix_AT_t {
	double a[2][MAX_M];
};

struct matrix_B_t{
	double a[MAX_M];
};

struct matric_A_AT_t {
	double a[MAX_M][MAX_M];
};

struct matric_AT_A_t {
	double a[2][2];
};

struct matric_AT_A_1_t {
	double a[2][2];
};

struct matric_AT_A_1_AT_t {
	double a[2][MAX_M];
};

struct x_t {
	double x;
	double y;
};

static struct matrix_A_t A;
static struct matrix_A_1_t A_1;
static struct matrix_B_t B;
static struct matrix_AT_t AT;
static struct matric_A_AT_t A_AT;
static struct matric_AT_A_t AT_A;
static struct matric_AT_A_1_t AT_A_1;
static struct matric_AT_A_1_AT_t  AT_A_1_AT;
static struct x_t X;

static int set_matrix_A_AT_B(struct point_t *pi, int num)
{
	num = (num > MAX_M) ? MAX_M : num;
	num--;

	int i;
	for(i = 0; i < num; i++) {
		/* set A */
		A.a[i][0] = pi[i].x - pi[num].x;
		A.a[i][1] = pi[i].y - pi[num].y;

		/* set AT */
		AT.a[0][i] = A.a[i][0];
		AT.a[1][i] = A.a[i][1];

		/* set B */
		B.a[i] = pow(pi[num].x, 2) - pow(pi[i].x, 2) 
			+ pow(pi[num].y, 2) - pow(pi[i].y, 2) 
			+ pow(pi[i].d, 2) - pow(pi[num].d, 2);
		B.a[i] /= -2;

		/*
		B.a[i] =  pow(pi[i].x, 2) - pow(pi[num].x, 2)
			+ pow(pi[i].y, 2) - pow(pi[num].y, 2)
			+ pow(pi[num].d, 2) - pow(pi[i].d, 2);
		printf("%lf = %lf - %lf + %lf - %lf + %lf - %lf\n",B.a[i],
			pow(pi[i].x, 2), pow(pi[num].x, 2),
			pow(pi[i].y, 2), pow(pi[num].y, 2),
			pow(pi[num].d, 2), pow(pi[i].d, 2));
		*/
	}

	printf("m: %d\n", num);
	printf("A: \n");
	for(i = 0; i < num; i++) {
		printf("%lf %lf\n", A.a[i][0], A.a[i][1]);
	}

	printf("AT: \n");
	for(i = 0; i < num; i++)
		printf("%lf ", AT.a[0][i]);
	printf("\n");
	for(i = 0; i < num; i++)
		printf("%lf ", AT.a[1][i]);
	printf("\n");

	printf("B: \n");
	for(i = 0; i < num; i++)
		printf("%lf\n", B.a[i]);

	return num;
}

static void mul_A_AT(int m)
{
	int i, j;
	printf("A_AT:\n");
	for(i = 0; i < m; i++) {
		for(j = 0; j < m; j++) {
			A_AT.a[i][j] = A.a[i][0] * AT.a[0][j] 
				+ A.a[i][1] * AT.a[1][j];
			printf("%lf ", A_AT.a[i][j]);
		}
		printf("\n");
	}

}

static void mul_AT_A(int m)
{
	int i, j;
	printf("AT_A: %d\n", m);
	for(i = 0; i < 2; i++) {
		for(j = 0; j < m; j++) {
			AT_A.a[i][0] += AT.a[i][j] * A.a[j][0];
			AT_A.a[i][1] += AT.a[i][j] * A.a[j][1];
		}
		printf("%lf %lf\n", AT_A.a[i][0], AT_A.a[i][1]);
	}
}

static int cal_A_1(int m)
{
	double denom = A.a[0][0] * A.a[1][1]
		- A.a[0][1] * A.a[1][0];
	if(denom == 0) {
		printf("demon == 0\n");
		return -1;
	}
	A_1.a[0][0] = A.a[1][1] / denom;
	A_1.a[0][1] = -A.a[0][1] / denom;
	A_1.a[1][0] = -A.a[1][0] / denom;
	A_1.a[1][1] = A.a[0][0] / denom;
	printf("A_1: %d\n", m);
	printf("%lf %lf\n%lf %lf\n", 
		A_1.a[0][0], A_1.a[0][1], 
		A_1.a[1][0], A_1.a[1][1]);
}

static int cal_AT_A_1(int m)
{
	double denom = AT_A.a[0][0] * AT_A.a[1][1] 
		- AT_A.a[0][1] * AT_A.a[1][0];
	printf("denom:%lf: %lf * %lf - %lf * %lf\n", denom, 
		AT_A.a[0][0], AT_A.a[1][1], 
		AT_A.a[0][1] ,AT_A.a[1][0] );

	if(denom == 0) {
		printf("demon == 0\n");
		return -1;
	}
	AT_A_1.a[0][0] = AT_A.a[1][1] / denom;
	AT_A_1.a[0][1] = -AT_A.a[0][1] / denom;
	AT_A_1.a[1][0] = -AT_A.a[1][0] / denom;
	AT_A_1.a[1][1] = AT_A.a[0][0] / denom;

	printf("AT_A_1: %d\n", m);
	printf("%lf %lf\n%lf %lf\n", 
		AT_A_1.a[0][0], AT_A_1.a[0][1], 
		AT_A_1.a[1][0], AT_A_1.a[1][1]);
}

static void cal_AT_A_1_AT(int m)
{
	int i, j;
	printf("AT_A_1_AT: %d\n", m);
	for(i = 0; i < 2; i++) {
		for(j = 0; j < m; j++) {
			AT_A_1_AT.a[i][j] = 
				AT_A_1.a[i][0] * AT.a[0][j] 
				+ AT_A_1.a[i][1] * AT.a[1][j];

			printf("%lf ", AT_A_1_AT.a[i][j]);
		}
		printf("\n");
	}
}

static void __get_x(int m)
{
	int i;
	for(i = 0; i < m; i++) {
		X.x += AT_A_1_AT.a[0][i] * B.a[i];
		X.y += AT_A_1_AT.a[1][i] * B.a[i];
	}
}

static void __get_x2()
{
	int i;
	for(i = 0; i < 2; i++) {
		X.x += A_1.a[0][i] * B.a[i];
		X.y += A_1.a[1][i] * B.a[i];
	}
	X.x /= 2;
	X.y /= 2;
}

static int flag = 0;
int Dtransform(double D[MAX_M][MAX_M],int dia,int n)
{
	int r,c;
	double temp;
	for(r=dia+1;r<n;r++)
	{
		if(D[r][dia]!=0)
		{
			for(c=dia;c<n;c++)
			{
				temp=D[r][c];
				D[r][c]=D[dia][c];
				D[dia][c]=temp;
			}
			flag=!flag;
			break;
		}
	}
	return r;
}

double Dcalcular(double D[MAX_M][MAX_M],int n)
{
	int dia,r,c,start;
	double temp,result=1;
	for(dia=0;dia<n;dia++)
	{
		if(fabs(D[dia][dia])<1e-6)
			start=Dtransform(D,dia,n);
		else
			start=dia;
		if(start==n)continue;
		for(r=start+1;r<n;r++)
		{
			if(fabs(D[r][dia])<1e-6) continue;
			temp=D[r][dia]/D[dia][dia];
			for(c=dia;c<n;c++)
				D[r][c]-=D[dia][c]*temp;
		}
	}
	for(dia=0;dia<n;dia++)
	{
		if(fabs(D[dia][dia])<1e-6)return 0;
		result*=D[dia][dia];
	}
	if(flag)result=-result;
	return result;
}

static int A_AT_NULL(int m)
{
	return !Dcalcular(A_AT.a, m);
}

int get_point(struct point_t *pi, int num, double *x, double *y)
{
	int i, m, ret;

	/*
	struct point_t *__pi = malloc(sizeof(struct point_t) * (num + 1));
	if(__pi == NULL) return -1;
	memcpy(__pi, pi, sizeof(struct point_t) * num);
	for(i = 0; i < num; i++) {
		__pi[num].x += pi[i].x;
		__pi[num].y += pi[i].y;
		__pi[num].d += pi[i].d;
	}
	__pi[num].x /= num;
	__pi[num].y /= num;
	__pi[num].d /= num;
	m = set_matrix_A_AT_B(__pi, num+1);
	*/


	m = set_matrix_A_AT_B(pi, num);
	cal_A_1(m);

	mul_A_AT(m);
	mul_AT_A(m);

	ret = cal_AT_A_1(m);
	if(ret < 0) return ret;

	cal_AT_A_1_AT(m);
	__get_x(m);
//	__get_x2(m);
	*x = X.x;
	*y = X.y;
	/*
free:
	free(__pi);
	*/
	return 0;
}

/*
 * pld: dissipation
 * d0: sample point distance
 * pld0: d0 point dissipation
 * */
double distance(double pld, double d0, double pld0, double r)
{
	double x = (pld - pld0) / (10 * r);
	double y = pow(10, x);
	return d0 * y;
}
